package net.helix.physics;

import org.lwjgl.util.vector.Vector3f;

public class Collision {
	static Vector3f p0 = new Vector3f();
	static Vector3f p1 = new Vector3f();
	static Vector3f p2 = new Vector3f();
	static Vector3f p3 = new Vector3f();
	static Vector3f p4 = new Vector3f();
	static Vector3f p5 = new Vector3f();
	static Vector3f p6 = new Vector3f();
	static Vector3f p7 = new Vector3f();
	static Vector3f[] cubeAsArray = new Vector3f[8];
	public static boolean rayIntersectCube(Vector3f rayPoint, Vector3f ray, float[][] cube){
		p0.x = cube[0][0];
		p0.y = cube[0][1];
		p0.z = cube[0][2];
		cubeAsArray[0] = p0;
		
		p1.x = cube[1][0];
		p1.y = cube[1][1];
		p1.z = cube[1][2];
		cubeAsArray[1] = p1;
	
		p2.x = cube[2][0];
		p2.y = cube[2][1];
		p2.z = cube[2][2];
		cubeAsArray[2] = p2;
		
		p3.x = cube[3][0];
		p3.y = cube[3][1];
		p3.z = cube[3][2];
		cubeAsArray[3] = p3;
		
		p4.x = cube[4][0];
		p4.y = cube[4][1];
		p4.z = cube[4][2];
		cubeAsArray[4] = p4;
		
		p5.x = cube[5][0];
		p5.y = cube[5][1];
		p5.z = cube[5][2];
		cubeAsArray[5] = p5;
		
		p6.x = cube[6][0];
		p6.y = cube[6][1];
		p6.z = cube[6][2];
		cubeAsArray[6] = p6;
		
		p7.x = cube[7][0];
		p7.y = cube[7][1];
		p7.z = cube[7][2];
		cubeAsArray[7] = p7;
		
		return rayIntersectCube(rayPoint, ray, cubeAsArray);
	}
	public static boolean rayIntersectCube(Vector3f rayPoint, Vector3f ray, Vector3f[] cube){
		/*    The cube MUST be defined in the order as follows:
		 *     1_______2
		 *     /      /|
		 *    /      / |
		 *  4/______/3 |
		 *   |(5)   |  /6
		 *   |      | /
		 *   |______|/
		 *  8        7
		 *   
		 */
		
		// Testing faces:
		// 124
		// 145
		// 438
		// 327
		// 216
		// 657
		
		int count = 0;
		
		if(rayIntersectQuad(rayPoint, ray, cube[0], cube[1], cube[3])){
			count++;
		}
		if(rayIntersectQuad(rayPoint, ray, cube[0], cube[3], cube[4])){
			count++;
		}
		if(rayIntersectQuad(rayPoint, ray, cube[3], cube[2], cube[7])){
			count++;
		}
		if(rayIntersectQuad(rayPoint, ray, cube[2], cube[1], cube[6])){
			count++;
		}
		if(rayIntersectQuad(rayPoint, ray, cube[1], cube[0], cube[5])){
			count++;
		}
		if(rayIntersectQuad(rayPoint, ray, cube[5], cube[4], cube[6])){
			count++;
		}
		System.out.println("Planes intersected " + count);
		return (count > 0);
	}
	static Vector3f va = new Vector3f();
	static Vector3f vb = new Vector3f();
	static Vector3f vc = new Vector3f();
	static Vector3f vd = new Vector3f();
	
	static Vector3f vnorm = new Vector3f();
	static Vector3f vnum = new Vector3f();
	
	static Vector3f plane4 = new Vector3f();
	
	static Vector3f v1_4 = new Vector3f();
	
	/**
	 * Checks if a line starting at rayPoint and going in direction ray
	 * will eventually intersect the quad defined with 3 points (the last point is assumed by making it a parallelogram)
	 * @param rayPoint - starting point of ray.
	 * @param ray - direction of ray.
	 * @param plane1 - first point of plane.
	 * @param plane2 - second point of plane.
	 * @param plane3 - third point of plane.
	 * @return Whether or not a collision was detected.
	 */
	public static boolean rayIntersectQuad(Vector3f rayPoint, Vector3f ray, Vector3f plane1, Vector3f plane2, Vector3f plane3){
		/*    The plane is defined as this:
		 *     1_______2 x ->
		 *     /      /  y
		 *    /  a   /   |
		 *(4)/______/3   v
		 *   4 is calculated from the other points.
		 */
		
		// plane defined by 3 points
		Vector3f.sub(plane2, plane1, va);
		Vector3f.sub(plane3, plane2, vb);
		
		Vector3f.add(plane1, vb, plane4);
		
		
		Vector3f.sub(plane4, plane1, vd);
		
		
		Vector3f.cross(va, vb, vnorm);
		// vc is now the normal of the plane
		
		Vector3f.sub(plane1, rayPoint, vnum);
		
		float numerator = Vector3f.dot(vnum, vnorm);
		float denominator = Vector3f.dot(ray, vnorm);
		
		if(denominator == 0){
			// parallel to plane... what do? 
			return false;
		}
		
		float d = numerator/denominator;
		
		if(d >= 0){ // since line goes out infinitely, we don't have d <= 1
			// find point on plane, ??
			Vector3f point = new Vector3f();
			Vector3f point2 = new Vector3f(ray.x * d, ray.y * d, ray.z * d);
			Vector3f.add(rayPoint, point2, point);
			// point is now the point where it intersects the plane.
			
			// Now I need to determine if it is in the bounds of this face.
			//point;
			System.out.println("point = "+point);
			
			Vector3f.sub(point, plane1, point);
			
			float x = Vector3f.dot(point, va)/va.length();
			float y = Vector3f.dot(point, vb)/vb.length();
			
			if(x >= 0 && x <= 1 && y >= 0 && y <= 1){
				return true;
			}
			
			
		}
		
		
		return false;
	}
	public static boolean rayIntersectTriangle(Vector3f rayPoint, Vector3f ray, Vector3f plane1, Vector3f plane2, Vector3f plane3){
		return false;
	}
}
